/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-06-00     YTR       the first version
 */

#define DBG_TAG "thread_d"
#define DBG_LVL DBG_LOG

#include "all.h"

float pitch,roll,yaw;       //欧拉角
rt_uint8_t mode = 0;

void mpu6050_rtt_entry(void *parameter){
    unsigned char rev_flag=0;
    uint8_t rev_number = 0;
    while(1){
        LED_Toogie();
        if((rev_flag=mpu_dmp_get_data(&pitch,&roll,&yaw))==0)
        {
            char buff[64];
            sprintf(buff,"pitch:%.2f roll:%.2f yaw:%.2f   mode:%d\n",pitch,roll,yaw,mode);
            rt_kprintf(buff);
            rev_number=0;
        }
        else{
            rev_number++;
            if(rev_number == 10){
                MPU6050_Init();
                rev_number=0;
            }
        }
        rt_thread_mdelay(100);
    }
}

void steering_rtt_entry(void *parameter){
    while(1){
        Robot_Mode(mode);
        rt_thread_mdelay(1000);
    }
}
void Mode_set(uint8_t value){
    mode = value;
}

void thread_init_function(void){
    rt_thread_t mpu6050_rtt = rt_thread_create("mpu6050", mpu6050_rtt_entry, NULL, 2048, 10, 5);

    if(mpu6050_rtt == RT_NULL){
        LOG_E("mpu6050_rtt create failed...\n");
        return;
    }

    rt_err_t ret = rt_thread_startup(mpu6050_rtt);
    if(ret != RT_EOK){
        LOG_E("mpu6050_rtt startup failed: %d\n", ret);
        rt_thread_delete(mpu6050_rtt);
        return;
    }

    LOG_D("mpu6050 thread started successfully\n");

    rt_thread_t steering_rtt = rt_thread_create("steering", steering_rtt_entry, NULL, 1024, 10, 5);

    if(steering_rtt == RT_NULL){
        LOG_E("steering_rtt create failed...\n");
        return;
    }

    rt_err_t ret1 = rt_thread_startup(steering_rtt);
    if(ret != RT_EOK){
        LOG_E("steering_rtt startup failed: %d\n", ret1);
        rt_thread_delete(steering_rtt);
        return;
    }

    LOG_D("steering thread started successfully\n");
}
